// Copyright (c) M5Stack. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.

#ifndef __M5_IMU_CLASS_H__
#define __M5_IMU_CLASS_H__

#include "I2C_Class.hpp"
#include "imu/IMU_Base.hpp"
#include <memory>

namespace m5
{
    enum imu_t
    {
        imu_none,
        imu_unknown,
        imu_sh200q,
        imu_mpu6050,
        imu_mpu6886,
        imu_mpu9250,
        imu_bmi270,
    };

    class IMU_Class
    {
    public:
        struct imu_3d_t
        {
            union
            {
                float value[3];
                struct
                {
                    float x;
                    float y;
                    float z;
                };
            };
        };

        struct imu_data_t
        {
            uint32_t usec;
            union
            {
                float value[9];
                imu_3d_t sensor[3];
                struct
                {
                    imu_3d_t accel;
                    imu_3d_t gyro;
                    imu_3d_t mag;
                };
            };
        };

        enum axis_t
        {
            axis_x_pos = 0,
            axis_x_neg = 1,
            axis_y_pos = 2,
            axis_y_neg = 3,
            axis_z_pos = 4,
            axis_z_neg = 5,
        };

        enum sensor_index_t
        {
            sensor_index_accel = 0,
            sensor_index_gyro = 1,
            sensor_index_mag = 2,
        };

        enum sensor_mask_t
        {
            sensor_mask_none = 0,
            sensor_mask_accel = 1 << sensor_index_accel,
            sensor_mask_gyro = 1 << sensor_index_gyro,
            sensor_mask_mag = 1 << sensor_index_mag,
        };

        // bool begin(I2C_Class* i2c = nullptr, board_t board = board_t::board_unknown);
        bool begin(I2C_Class* i2c = nullptr);
        bool init(I2C_Class* i2c = nullptr) { return begin(i2c); }

        sensor_mask_t update(void);

        void getImuData(imu_data_t* imu_data);

        const imu_data_t& getImuData(void)
        {
            getImuData(&_last_data);
            return _last_data;
        }

        // 軸の順序を指定する。デフォルトはX+,Y+,Z+
        bool setAxisOrder(axis_t axis0, axis_t axis1, axis_t axis2);

        // 軸の順序を右手系で指定する。最初の２軸のみ指定し、３軸目は省略
        bool setAxisOrderRightHanded(axis_t axis0, axis_t axis1);

        // 軸の順序を左手系で指定する。最初の２軸のみ指定し、３軸目は省略
        bool setAxisOrderLeftHanded(axis_t axis0, axis_t axis1);

        bool getAccel(float* ax, float* ay, float* az);
        bool getGyro(float* gx, float* gy, float* gz);
        bool getMag(float* mx, float* my, float* mz);
        bool getAccelData(float* ax, float* ay, float* az) { return getAccel(ax, ay, az); }
        bool getGyroData(float* gx, float* gy, float* gz) { return getGyro(gx, gy, gz); }
        bool getGyroMag(float* mx, float* my, float* mz) { return getMag(mx, my, mz); }
        bool getTemp(float* t);

        bool isEnabled(void) const { return _imu != imu_none; }

        imu_t getType(void) const { return _imu; }

        // 実装予定
        // void getAhrsData(float *pitch, float *roll, float *yaw);

        // 廃止
        // void setRotation(uint_fast8_t rotation) { _rotation = rotation & 3; };

        bool setINTPinActiveLogic(bool level);

        // 各センサの自動オフセット調整機能の強さを指定する。 0=自動調整なし 1~255=自動調整あり
        void setCalibration(uint8_t accel_strength, uint8_t gyro_strength, uint8_t mag_strength);

        // 現在のオフセット調整値をNVSに保存する
        bool saveOffsetToNVS(void);

        // NVSからオフセット調整値を読み込む
        bool loadOffsetFromNVS(void);

        // オフセットデータをクリアする
        void clearOffsetData(void);

        // OffsetData は RawData<<16 スケール(16bit固定小数扱い)
        void setOffsetData(size_t index, int32_t value);

        int32_t getOffsetData(size_t index);

        int16_t getRawData(size_t index);

    private:
        struct offset_point_t
        {
            union
            {
                int32_t value[3];
                struct
                {
                    int32_t x;
                    int32_t y;
                    int32_t z;
                };
            };
            union
            {
                int32_t prev_value[3];
                struct
                {
                    int32_t prev_x;
                    int32_t prev_y;
                    int32_t prev_z;
                };
            };
            union
            {
                int32_t avg_value[3];
                struct
                {
                    int32_t avg_x;
                    int32_t avg_y;
                    int32_t avg_z;
                };
            };
            float radius;
            float tolerance;
            uint16_t noise_level;
            uint8_t average_shifter; // 前回値への移動平均設定
            uint8_t stillness;       // 移動量の少なさ(変動が大きい時0になり、静止時に255に近付く)
            uint8_t strength;        // キャリブレーションの強さ(ユーザー指定)

            std::uint_fast8_t updateStillness(const IMU_Base::point3d_i16_t& dst);
            void calibration(void);
            inline void setValue16(size_t index, int16_t val) { value[index] = val << 16; }
            inline int32_t getValue16(size_t index) const { return value[index] >> 16; }
        } __attribute__((__packed__));

        struct imu_offset_data_t
        {
            union
            {
                offset_point_t sensor[3];
                struct
                {
                    offset_point_t accel;
                    offset_point_t gyro;
                    offset_point_t mag;
                };
            };
        };

        void _update_convert_param(void);
        void _update_axis_order(void);
        void _proc_calibration(void);

        // update成功時のマイクロ秒情報
        uint32_t _latest_micros;

        // センサのインスタンス保持用 ([0]=加速度+ジャイロ / [1]=地磁気)
        std::unique_ptr<IMU_Base> _imu_instance[2];

        // 生の値を測定値に変換するための補正値
        IMU_Base::imu_convert_param_t _convert_param;

        // updateで得た最新の生値
        IMU_Base::imu_raw_data_t _raw_data;

        // オフセット補正用の値
        imu_offset_data_t _offset_data;

        // 最後に得たセンサ値
        imu_data_t _last_data;

        // 最後に動きがあった時のusec
        uint32_t _moving_micros;

        // ユーザ側で任意の軸割当を行うための設定項目
        uint8_t _assign_axis_x;
        uint8_t _assign_axis_y;
        uint8_t _assign_axis_z;

        sensor_mask_t _calibration_flg;

        sensor_mask_t _has_sensor_mask;

        imu_t _imu = imu_t::imu_unknown;

        // 機種別設定とユーザー設定の両方を反映した後のデータ取得順の値
        uint32_t _axis_order_3bit_x9;

        enum internal_axisorder_t : uint8_t
        {
            axis_invert_x = 1,
            axis_invert_y = 2,
            axis_invert_z = 4,
            axis_order_shift = 3,
            axis_order_xyz = 0 << axis_order_shift,
            axis_order_xzy = 1 << axis_order_shift,
            axis_order_yxz = 2 << axis_order_shift,
            axis_order_yzx = 3 << axis_order_shift,
            axis_order_zxy = 4 << axis_order_shift,
            axis_order_zyx = 5 << axis_order_shift,
        };

        // 機種別の軸の不一致を統一するための設定項目(システム自動設定)
        internal_axisorder_t _internal_axisorder_fixed[3] = {
            (internal_axisorder_t)0, (internal_axisorder_t)0, (internal_axisorder_t)0};

        // ユーザーによる設定値
        internal_axisorder_t _internal_axisorder_user = (internal_axisorder_t)0;
    };

    typedef IMU_Class::imu_3d_t imu_3d_t;
    typedef IMU_Class::imu_data_t imu_data_t;
} // namespace m5
#endif